//---------------------------------------------------------------------------
// Copyright (c) 2011 Texas Instruments, Inc.
// This is an unpublished work created in the year stated above.
// Texas Instruments owns all rights in and to this work and
// intends to maintain and protect it as an unpublished copyright.
// In the event of either inadvertent or deliberate publication,
// the above stated date shall be treated as the year of first
// publication. In the evxfent of such publication, Texas Instruments
// intends to enforce its rights in the work under the copyright
// laws as a published work.
//----------------------------------------------------------------------------
// Filename                : drv922xx_trapezoidal_sensors_v0.11.c
//
// Functional Description  : Brushless DC Motor Control Based on the DRV9410.
//                           Performs Trapazoidal commutation of a BLDC motor,
//                           both standard and complimentary with dead time.
//                           This code also reads and stores the Current at the 
//                           midpoint of the high side PWM pulse and just before 
//                           the falling edge of the high side PWM pulse.
//
// Revision History        : 
//                           v0.3		Added f/w iLim, h/w iLim, 
//                                      complementary switch, 100% duty cycle mode
//							 v0.4		Added reverse mode. Fixed 100% duty mode to be driven via throttle only.
//										Added UART support and messaging protocol
//							 v0.5		Added application parameters that are configurable
//							 v0.6		Fixed reverse function
//							 v0.7		Fixed complementary mode - control when P2OUT/P2SEL
//							 v0.8		Added UART support and messaging protocol, fixed support for 1p2
//										(MONTEREY_ROM undefined in DRV922XX.h).
//							 v0.9		Minimized time between hall sensor state change and commutation - causing over-current
//							 v0.10		Fixed time between hall sensor state change and commutation by doing all commutator
//									    and duty cycle adjustments in same place (in sample throttle). Makes minimal amount
//										of time between edge changes in hall sensor and actual commutation.
//						     v0.11		Made AFE register settings persistent, like other UART-accessable data. Debug UART 
//										communication.
//----------------------------------------------------------------------------

// NOTE THAT YOU MUST HAVE CCS COMPILER OPTIMIZATION SET TO 2 TO OPTIMIZE INLINE CODE FOR THE BEST PERFORMANCE
#include "DRV922xx_trapezoidal_sensored.h" //#include "DRV922XX.h"
#include "message_protocol_application.h"
#include "afe_data.h"

//
// Globals
//
unsigned char update_pwm_flag = 0;										/*!< set when time to update PWM duty cycle */
unsigned char complementary_pwm_saved = 0;								/*!< Current value of complementary PWM mode */

//int reverse_drive = 0;										/*!< Set to 1 if we are driving motor in reverse */
int reverse_drive = 0;//1; 

int just_changed_direction = 0;
int target_drive_direction = DIR_FORWARD;//DIR_STOP;
int process1msFlag = 1;
int process10msFlag = 1;
int sample1msADCsFlag = 1;
int hallIsrFlag = 1;
int mode = MODE_OFF;
unsigned int brakeState = BRAKE_OFF;
unsigned int isMoving = IS_NOT_MOVING;

unsigned char led = 0;													/*!< When set to 1, enables LED for debugging */
unsigned char complementary_pwm = 0;									/*!< Set to 1 when PWM in complementary mode */

volatile char saved_com_state = 0x00; 									/*!< initial state will be guaranteed to mismatch */
unsigned char new_com_state = 0x00;   									/*!< used to store current com state in update_com() */
unsigned char comm_tbl_index;											/*!< last known commutator table index */
unsigned char update_commutator = 0;									/*!< flag to specify when to update commutator */
unsigned int timeOffSec = 0;
unsigned int speed = 0; 
unsigned int speedFiltered = 0;
unsigned int pwmTicks = 0;

volatile unsigned char spi_rx_data = 0;									/*!< used to store the received data from the SPI */

volatile unsigned int pwm_duty = 0;										/*!< used to store calculated PWM duty cycle */

volatile unsigned char throttle_voltage_high = 0;						/*!< used to store MSB of throttle voltage when reading ADC (10b result) */
volatile unsigned char throttle_voltage_low = 0;						/*!< used to store LSB of throttle voltage when reading ADC (10b result) */

volatile unsigned char MaxiCurrent_high = 0;							/*!< used to store MSB of max current when reading ADC (10b result) */
volatile unsigned char MaxiCurrent_low = 0;								/*!< used to store LSB of max current when reading ADC (10b result) */

// Data for Messaging Protocol Common interface

unsigned int msg_device_type = MSG_ID_SENSE_120_DRV92210;
unsigned int msg_fw_version = 0x0001; // MSB|LSB

// Function prototypes
void main_init();
void peripheral_init();													/*!< Initializes all peripherals */
//void pwm_disable();														/*!< Disables all 6 PWM outputs (low and high side) for all three phases */
//void update_com(void);													/*!< Updates the high and low side port state, based on commutation */
//void sample_throttle(void);												/*!< Samples the throttle voltage, filters result and stores */
//void sample_avg_current(void);											/*!< Samples the current and stores it as the average (call at PWM on mid) */
//void sample_peak_current(void);											/*!< Samples the current and stores it as the peak (call at PWM on end) */
//void blink_led(unsigned int delay);										/*!< Blinks P2.0 with the given delay time and does not return */ 

// Interrupt handlers

//interrupt void hall_change_isr(void);									/*!< Called when a hall sensor change is detected */
//interrupt void measure_isr(void);										/*!< Called right before the mid-point and end of the PWM duty for sampling current */
//interrupt void afe_over_current_isr(void);								/*!< Called when PORT1 interrupt received for HW OC [P1.1] */
//interrupt void coarse_timer_tick(void);									/*!< Called every 2ms to signify when to update PWM duty cycle */
//interrupt void invalid_int_isr(void);									/*!< Called for invalid interrupts for debugging */
void main_init()
{
	    volatile int i;
 
    _disable_interrupt();

	// Set clock control and FRAM control
	// NOTE: There is a bug in the micro-controller (any based on FRAM) where there is contention between the power-up setting of CCSCTL0 and FRCTL0
	// if FRCTL0 is changed to 0xxx11 while the mclk is set to divide by 8. This could cause some corruption when fetching FRAM locations. To avoid
	// this scenario, make sure to have the code below that sets the MCLK to /4 before FRCTL0 is written and then set CCSCTL0 to the proper value
	CCSCTL0 = SCSKEY + WSEN +  DIVA__1 + DIVS__1 + DIVM__4;							// ACLK/SMCLK = MainClock, MCLK = SCLK/4
	FRCTL0 = FWKEY + NPRECHG0 + NACCESS0 ;											// Write Password + Setting 1,1
	CCSCTL0 = SCSKEY + WSEN +  DIVA__1 + DIVS__1 + DIVM__1;							// ACLK/SMCLK/MCLK = MainClock/1 (25MHz)

    // Disable Watchdog Timer - TBD: we may want to use this during operation
//    WDTCTL = WDTPW + WDTHOLD;
	WDTCTL = WDTPW + WDTIS_4 + WDTSSEL__SMLCK; // WDT FREQ = SMCLK / 512k = 25MHz / 512k = 48.8 Hz ==> 20.48 ms
    
    // Disable inadvertent resets when noisy ~RST signal
    SFRRPCR |= SYSNMI;
    
    AppStatus |= StatusInit;
    
    // Initialization routines
    peripheral_init();

    // Read Icomp/Predrve to make sure pre-drivers are on
    spi_rx_data = drv922xxSpiRead(SPI_ICOMP_READ);
    if ((spi_rx_data) & 0x80 != 0x80)
        while (1);


    // Initialize for first time to force com_update to sync to halls sensor reading
    saved_com_state = 0;

    // Read version number and see if valid to make sure AFE communication is working
    spi_rx_data = drv922xxSpiRead(SPI_DAC1_READ);
#ifdef MONTEREY_ROM
    if ((spi_rx_data & 0xF0) != 0x30)							// 1p3 only
#else
	if ((spi_rx_data & 0xF0) != 0x20)							// 1p2 only
#endif
        while (1);
        
  	AppFanOff = 0;
  	timeOffSec = 0;
  	AppStatus &= ~StatusInit;
  	if (getTargetDirection() == DIR_REVERSE)
  	{
  		reverse_drive = 1;
  	}   
    led = 1;
    P2OUT |= led;
//    led ^= 1;
//		if (!led)
//			P2OUT &= ~0x01;
//		else
//			P2OUT |= 0x01;

    _enable_interrupt();
}
/*
 *  \fn void peripheral_init()
 *
 *  \brief Initializes digital IO ports, timers, SPI, ADC and I2C
 */
void peripheral_init()
{
    led = 0;

    // Ports

    P1OUT = 0x00;                           			// Set output state of P1.4, P1.5, P1.6 to low
    P1DIR = 0x70;                           			// P1.4, P1.5, P1.6 outputs for debug. P1.0 output for ADC START
   	P1REN = 0x80;                           			// SPARE1 Input on EVM, Make pulldown

    P2OUT = 0x00;                           			// Set output state of P2.1 - P2.6 (PWM output to AFE) to low
    P2DIR = 0x7f;                           			// Bit0 = LED, Bits1-6 = PWM, Bit7 = SPARE2
    P2SEL = 0x00;                           			// Bits1-6 = PWM driver by module
    P2REN = 0x80;                           			// SPARE2 Input on EVM, Make pulldown

#ifdef MONTEREY_ROM
    P3DIR = BIT7;				             			// Set SPI reset as output (P3.7 to AFE)
    P4OUT &= ~(BIT1 | BIT0);							// Set output state of P4.0, P4.1 to low
    P4DIR = BIT3 | BIT2 | BIT1 | BIT0;					// P4.0, P4.1 outputs for debug, P4.2-P4.3 for SPI output (SIMO, CLK)
    P4SEL = BIT4 | BIT3 | BIT2;							// P4.2-P4.4 set to use peripheral 
#else
    P3DIR = BIT7 | BIT5 | BIT4;             			// Set SPI reset as output (P3.7 to AFE)
	P3SEL = BIT6 | BIT5 | BIT4;             			// Non-port driven (driven by USCI block)
    P4OUT = 0x00;										// Set output state of P4.0, P4.1 to low
    P4DIR = BIT1 | BIT0;								// P4.0, P4.1 outputs for debug
    P3OUT = 0x00;										// Set output state of P4.0, P4.1 to low
#endif
    P3OUT = BIT7;                           			// Hold AFE SPI in reset (P3.7 high)

    // UCSI/SPI
 	USCI_CTL0_REG |= UCSWRST;                  			// Hold SPI peripheral in reset on digital controller during config
    USCI_CTL0_REG = UCCKPH | UCMSB | UCMST | UCSYNC;	// PHS: data sampled first edge, changed 2nd edge,
    													// Polarity: active high, MSB first, 8-bit mode, slave,
    													// 3-pin SPI, Sync mode (needs to be set for SPI)                                                    
    USCI_CTL1_REG |= UCSSEL_1;                 			// ACLK source                                            
    USCI_CTL1_REG &= ~UCSWRST;                 			// Release USCI from reset after SPI configuration complete
    P3OUT &= ~BIT7;                           			// Release AFE SPI slave from reset (P3.7 low) after master config complete
     
    // UART initialization and register for interrupts for Rx data
    uart_init();
    msg_protocol_init();
    
    // SPI/Analog init
    afe_data_init();
    afe_data_write_all();
    
    // BUG: Force a read of MISC1 to reset P1.1 (h/w OC), so we can see OC interrupt transitions during over-current events from the AFE
    spi_rx_data = drv922xxSpiRead(SPI_MISC1_READ);
   
    // Timers
    
	// Timer B configuration - non-complementary with 0% duty cycle by default
	
	TBCCTL1 = TBCCTL3 = TBCCTL5 = OUTMOD_2 | CLLD_1; 	// Load TBCL1 on TBR==TBCL0, active above threshold
	
    if (complementary_pwm)
    {
    	AppStatus |= StatusCmplMode;
    	TBCCTL2 = TBCCTL4 = TBCCTL6 = OUTMOD_6 | CLLD_1;
    }
    else
    {	AppStatus &= ~StatusCmplMode;
    	TBCCTL2 = TBCCTL4 = TBCCTL6 = OUTMOD_0;
    }
    
	TBCCR0  = CCR_PERIOD_TIME;									// Initialize the PWM cycle for up/down counter (0% duty) 
    
    // Measurement timer
    TA3CCR0 = TBCCR0*2-1; 								// Initial the PWM cycle for up counter to be same cycle as Timer B
    TA3CCR1 = TBCCR0*2;									// PWM rising edge
    TA3CCR2 = 0;										// PWM falling edge
    
    TA3CCTL1 = OUTMOD_6;  								// Reset/Set, Load TA3CL1 on TA3R==TA3CL0
    TA3CCTL2 = OUTMOD_6;  								// Reset/Set, Load TA3CL2 on TA3R==TA3CL0
    
	// Coarse 1ms timer
	TA1EX0 = TAIDEX_7;									// Timer A1 extended divider (/8) for coarse 1ms timer (2.56 us/tick)
	TA1CCR0 = CCR_TICKS_1MS;							// 1ms timer ticks
    
    // Set all PWM duty cycles to 0% Duty on startup
    TBCCR1 = TBCCR2 = TBCCR3 = 
             TBCCR4 = TBCCR5 = CCR_PERIOD_TIME;  		// Set initial duty cycle (0%)
    
	// Configure/start timers
	
	TA1CTL = TASSEL_2 | TACLR_L | MC__UP | ID__8;		// Coarse 1ms timer	
	TA3CTL = TASSEL_2 | TACLR_L | MC__UP ;				// Measurement timer for PWM rising edge (SMCLK, up mode, clear) Make sure to enable right before Timer B (these should be sync'd)
    TBCTL = TBSSEL_2 | TBCLR_L | MC__UPDOWN ; 			// PWM output timer (SMCLK, up/down mode, clear)

    __delay_cycles(CCR_PERIOD_TIME*2);					// Delay to make sure all PWM signals are in sync (2 switching periods)


	// Enable timer interrupts
	TA1CCTL0 |= CCIE;									// Enable Timer A1 0 interrupt (1ms coarse timer)
//    TA3CCTL1 |= CCIE;  									// Enable Timer A3 1 interrupt (PWM rising edge)
#if !TEST_WITH_NO_CURRENT_SAMPLING
    TA3CCTL2 |= CCIE;									// Enable Timer A3 2 interrupt (PWM falling edge)
#endif	
    // Enable Hall interrupts for P3.1-P3.3 (HS1-HS3)
    P3IE = 0x0e;
    
    // Enable Hardware over-current interrupt (P1.1)
    P1IES = 0;											// P1.1 low-to-high transition
    P1IFG &= ~BIT1;										// Clear interrupt flag for P1.1
    P1IE = BIT1;										// Enable P1.1 interrupts (HW OC)
}
unsigned int getTargetDirection(void)
{
	if (UI_REVERSE_BTN_APPLIED)				// Reverse Drive? 
	{
		target_drive_direction = DIR_REVERSE;
	}
	else													// Forward Drive
	{
		target_drive_direction = DIR_FORWARD;
	}
	return target_drive_direction;
}
unsigned int directionChangeDetector(void)
{
	if (target_drive_direction == DIR_REVERSE) 
	{
		if (reverse_drive == 0)			// Previously in forward mode, reset things
		{
			pwm_disable();
			just_changed_direction = 1;
		}
		else 
			AppStatus |= StatusReverse;
	}
	else if (target_drive_direction == DIR_FORWARD){
		if (reverse_drive == 1) {
			pwm_disable();
			just_changed_direction = 1;
		}	
		else 
			AppStatus &= ~StatusReverse;
	}		
	return	just_changed_direction;
}
void directionChangeHandler(void)
{
	just_changed_direction = 1;
//	update_commutator = 0;
	pwm_disable();
	if (timeOffSec++ == 10000){
		timeOffSec = 0;
//		update_commutator = 1;
		just_changed_direction = 0;
    	if (target_drive_direction == DIR_REVERSE) 
		{
			reverse_drive = 1;
			AppStatus |= StatusReverse;
		}
		else if (target_drive_direction == DIR_FORWARD){
			reverse_drive = 0;	
			AppStatus &= ~StatusReverse;
		}			
	}
}
unsigned int getBrakeState(void)
{
	if ( (UI_BRAKE_APPLIED) || (AppFanOff) ) 
	{
		brakeState = BRAKE_ON;
		return BRAKE_ON;
	}
	brakeState = BRAKE_OFF;
	return BRAKE_OFF;
}
void ledBlinking1ms(void)
{
	static int led_blink_interval = 0;
	if ((AppStatus & StatusBrakeOn) == StatusBrakeOn)
	{
		if (AppMotorState == 1)
		{
			if (++led_blink_interval >= 50)
			{
				led_blink_interval = 0;
//				pwm_blink_led();
			}
		}
		else
		{
			if (++led_blink_interval >= 100)
			{
				led_blink_interval = 0;
//				pwm_blink_led();
			}
		}	
	}
	else if ((AppStatus & StatusBemf) != 0)
	{
//		static int turn = 0;
		if (++led_blink_interval >= 500)
		{
			led_blink_interval = 0;
//			pwm_blink_led();
//			if (turn == 0)
//			{
//				if (uart_transmitting_message == 0)
//				{
//					send_cmd(CmdMaxCurrent, AppMaxCurrent);
//				}
//				turn = 1;
//			}
//			else
//			{
//				if (uart_transmitting_message == 0)
//				{
//					send_cmd(CmdSpeed, AppSpeed);
//				}
//				turn = 0;
//			}
		}
//		if (led_blink_interval == 100)
//		update_pwm_flag = 1;
	}
	else 
	{
		if (++led_blink_interval >= 1000)
		{
			led_blink_interval = 0;
//			pwm_blink_led();
		}
//		update_pwm_flag = 1;
	}
}
void uart_breakSynch(void)
{
	if (UCA0STAT == 0)
	{
		UCA0STAT = 0;
	}
	else 
	{
		if ((UCA0STAT & UCBRK) != 0)
		{
			UCA0CTL1 &= ~UCDORM;								// Allow generation of UCRXIFG for all received bytes
			UCA0STAT &= ~UCBRK;									// Clear the break condition
		//	while (!(UCA0IFG & UCTXIFG));
		//		UCA0TXBUF = cmdMsg_breakSynch;
				send_breakSynch_ACK();
		}
	}
}
void process1ms(void)
{
	getBrakeState();
	#if !MSP_CONTROL
	getTargetDirection();
	#endif

	switch (mode)
	{
		case MODE_OFF:
			isMoving = IS_NOT_MOVING;
			pwmTicks = 0;
			if (getBrakeState() == BRAKE_ON)
       		{
//	        	led = 1;
//	        	P2OUT |= led;
	            pwm_disable();
	            throttle_reset();
	            AppStatus |= StatusBrakeOn;       
	            break;
        	} 
			if (throttleGetState() == THROTTLE_ON)
			{
				mode = MODE_ON;
				update_com();
				break;
			}
			pwm_disable();
			
		break;
		case MODE_ON:
			if (getBrakeState() == BRAKE_ON)
       		{
//	        	led = 1;
//	        	P2OUT |= led;
	        	// If we are in hardware over-current, then read MISC1 to clear condition
	        	if ((AppStatus & StatusHwOc) != 0)
	        		WDTCTL = 0xDEAD; // Reset controller by writing invalid password to WDT
	            pwm_disable();
	            AppStatus |= StatusBrakeOn;       
	            throttle_reset(); 
	            mode = MODE_OFF;
        	} 
			if (directionChangeDetector() == 1)
			{
				mode = MODE_DIRECTION_CHANGE;
				directionChangeHandler();
				break;
			}
			if (throttleGetState() == THROTTLE_OFF)
			{
				P2OUT = led;
				pwm_disable();
				mode = MODE_OFF;
				break;
			}
		break;
		case MODE_DIRECTION_CHANGE:
			if (just_changed_direction == 1)
			{
				directionChangeHandler();
			}
			else
			{ 
				mode = MODE_OFF;
//				update_com();
				throttle_reset();
			}
		break;
	}
//	update_pwm_flag = 1;
 	ledBlinking1ms();
 
 	uart_breakSynch();
 	
//	P1OUT |= BIT6;
//	send_cmd(CmdStatus, 0x7799);
//	uart_tick_1ms();
//	P1OUT &= ~BIT6;
}
/*
 *  \fn void main()
 *  \brief Main program
 */
void sample_temperature(void)
{
	AppTemperature = drv922xxAdcReadTemperature();
}
void main(void)
{
//	static unsigned int turn = 0;
	static unsigned int msDelay = 0;
	main_init();
  	
  	while (1)
    {   
		uart_tick_1ms();
		if (process1msFlag == 1 )
		{
			
			process1msFlag = 0;
			if (msDelay++ == 1)
			{
				throttle_filter();
				msDelay = 0;
			}
			process1ms();
			
				 	 	P2OUT |= BIT0;
			if (uart_transmitting_message == 0)
			{
				send_dataStream(CmdCurrent);
	//			P2OUT |= BIT0;
//				if (turn == 0)
//				{
//					turn = 1;
//	 	 			send_cmd(CmdCurrent, AppCurrentFiltered);
//				}
//				else
//				{
//					turn = 0;
//					send_cmd(CmdPwmDuty, pwm_duty);
//				}
	 //	 		P2OUT |= BIT0;
			}
//			else
//			{
	//			P2OUT |= BIT0;
//	    		uart_tick_1ms();
	//    		P2OUT |= BIT0;
//			}
			if (process10msFlag == 1)
			{
				process10msFlag = 0;
				__no_operation();
			}
	    	P2OUT &= ~BIT0;
		}
		uart_tick_1ms();
		
		if (hallIsrFlag == 1)
		{
			hallIsrFlag = 0;
			P3IE = 0x00;  						// Disable Hall Interrupts
		    update_com();
		    P3IE = 0x0e; 						// Enable Hall Interrupts
		}
    	// Check for brake asserted (P4.5 low)
        
   
 //       else
 //       {        
 //           // Check if brake was previously asserted
 //           if ((AppStatus & StatusBrakeOn) != 0)
 //           {
  //              AppStatus &= ~StatusBrakeOn;
 //               saved_com_state = 0;
  //              led = 0;
//                P2OUT = 0x00 | led; // Make sure that we don't assert pre-drivers when we disable PWM for dead time control below
 //               sample_throttle();		// Force a sample of throttle, in case it moved
//            }
       
       		// Check to see if throttle voltage too low
//       		if (throttleGetState() == THROTTLE_OFF)
//				pwm_disable();
//			else
//			{
			         
				// Check the hall sensor state. If there is a change from the last state, reconfigure the pre-driver
//	            P3IE = 0x00;  // Disable Hall Interrupts
//	            P3IFG = 0x00;
	// 			_disable_interrupt();
//	            update_com();
	 //           _enable_interrupt();
//	            P3IE = 0x0e; // Enable Hall Interrupts
//			}
//        } // if (no brake, no OC)
    }  // end of while loop (polling)
}  // end of main loop
